#define ETA 0.5 

template<class T, class Resample>
class PF
{
	public: 
		PF(int M, T* Model, Resample* R, mat theta, int t)
		{
			_M=M;
			_Model=Model;
			_p=_Model->Get_p();
			mat X(_M,_p);
			_X=X;
			_R=R;
			_T=t;
			_W=new double[_M];
			mat F(M,_T);
			_theta=theta;
			_Z=0;
			_Filtered=F;
			mat U(_T,1);
			Me=U;
		}

		void Init()
		{
			for(int i=0;i<_M;i++)
			{
				_X(i,span::all)=(_Model->rm(_theta.t())).t();	
			}
			Normalize();
		}
		void Step(int t,mat theta)
		{
			//cout << Ess_W(_W,0) <<" ";
			_r=0;
			if(Ess_W(_W,0)<(ETA*_M))
			{
				_r=1;
				mat v=growingvect(_M);
				double *w=new double[_M];
				for(int j=0;j<_M;j++){
					 w[j]=exp(_W[j]);
		
				}
				(*_R)(&_M,w,v);
				delete[] w;
				
				Arangemat(_X,v);
				Normalize();//poids fixé a 1
				//cout << mean(_X,0);
			//	Write();
			}
			mat XH=_Model->rq(theta.t(),_X,_M);
		//cout << mean(XH,0);
			Norm();
			_Model->Weights(_W,XH,_X,theta.t(),t);
			double ess=Ess_W(_W,1);
			_X=XH;
			Me(t,0)=mean2(_X,_W);

		}
		void Filter(void)
		{
			Init();
			for(int i=0;i<_T;i++)
			{
				Step(i,_theta);
			}
		}
		void Norm(void)
		{
			double sum=0;
			for(int i=0;i<_M;i++)
			{
				double t=log_add(sum,_W[i]);
				sum=t;
			}
			for(int i=0;i<_M;i++)
			{
				_W[i]=_W[i]-sum;
			}
		}
		void Normalize(void)
		{
			for(int i=0;i<_M;i++)
			{
				_W[i]=0;//-log(_M);
			}
		}
		double Ess_W(double *w, int tf)//renormalisation???
		{
			double *foo= new double[_M];
			//double *foo2= new double[_M];
			double sum=w[0];
			for(int i=1;i<_M;i++)
			{
				double t=log_add(w[i],sum);
				sum=t;
				//sum2+=exp(w[i]);
			}
			//cout << "sum " << sum-log(sum2);

			if(tf){
				if(_r){sum+=-log(_M);}
				_Z+=sum;
				
			}
			//cout << "the sum "<< sum;
			
			for(int j=0;j<_M;j++)
			{
				foo[j]=(w[j]-sum);
			//	foo2[j]=exp(w[j])/sum2;
				//cout << exp(foo[j])-exp(w[j])/sum2 << "\n";
			}

			sum=2*foo[0];
			for(int i=1;i<_M;i++)
			{
				double t=log_add(sum,2*foo[i]);
				sum=t;
			//	sum2+=foo2[i]*foo2[i];
			}
			delete[] foo;
			//delete[] foo2;
			//cout << "logESS "<< -sum;
			return exp(-sum);
		}

		double Get_Z(void){
			if(_Z>0)
			{
				cout << "log of a probability can't be >0";
			}
			return _Z;
		}
		mat GetMe(void)
		{
			return Me;
		}

	private:
		T* _Model;
		Resample* _R;
		mat _X;
		mat Me;
		int _r;
		int _M; 
		int _T;
		int _p;
		double _Z;
		double *_W;
		mat _Filtered;
		mat _theta;
};

